/*******************************************************************************
*                                                                              *
*   PrimeSense NITE 1.3 - Point Viewer Sample                                  *
*   Copyright (C) 2010 PrimeSense Ltd.                                         *
*                                                                              *
*******************************************************************************/

#include "PointDrawer.h"
#include <nite/XnVDepthMessage.h>
#include <nite/XnVHandPointContext.h>
#include <GL/glut.h>


// Constructor. Receives the number of previous positions to store per hand,
// and a source for depth map
XnVPointDrawer::XnVPointDrawer(XnUInt32 nHistory, xn::DepthGenerator depthGenerator) :
    XnVPointControl("XnVPointDrawer"),
    m_nHistorySize(nHistory), m_DepthGenerator(depthGenerator), m_bDrawDM(false), m_bFrameID(false)
{
    m_pfPositionBuffer = new XnFloat[nHistory*3];
    this->handFound = false;
}

// Destructor. Clear all data structures
XnVPointDrawer::~XnVPointDrawer()
{
    std::map<XnUInt32, std::list<XnPoint3D> >::iterator iter;
    for (iter = m_History.begin(); iter != m_History.end(); ++iter)
    {
        iter->second.clear();
    }
    m_History.clear();

    delete []m_pfPositionBuffer;
}

// Change whether or not to draw the depth map
void XnVPointDrawer::SetDepthMap(XnBool bDrawDM)
{
    m_bDrawDM = bDrawDM;
}
// Change whether or not to print the frame ID
void XnVPointDrawer::SetFrameID(XnBool bFrameID)
{
    m_bFrameID = bFrameID;
}

// Handle creation of a new hand
static XnBool bShouldPrint = false;
void XnVPointDrawer::OnPointCreate(const XnVHandPointContext* cxt)
{
    printf("** %d\n", cxt->nID);
    // Create entry for the hand
    m_History[cxt->nID].clear();
    bShouldPrint = true;
    OnPointUpdate(cxt);
    bShouldPrint = true;
    this->handFound = true;
}
// Handle new position of an existing hand
void XnVPointDrawer::OnPointUpdate(const XnVHandPointContext* cxt)
{
    // positions are kept in projective coordinates, since they are only used for drawing
    this->pt3D.X=cxt->ptPosition.X;
    this->pt3D.Y=cxt->ptPosition.Y;
    this->pt3D.Z=cxt->ptPosition.Z;

    //printf("Hand is at (%f, %f, %f) \n", cxt->ptPosition.X, cxt->ptPosition.Y, cxt->ptPosition.Z);

    XnPoint3D ptProjective(cxt->ptPosition);

    if (bShouldPrint)printf("Point (%f,%f,%f)", ptProjective.X, ptProjective.Y, ptProjective.Z);
    m_DepthGenerator.ConvertRealWorldToProjective(1, &ptProjective, &ptProjective);
    if (bShouldPrint)printf(" -> (%f,%f,%f)\n", ptProjective.X, ptProjective.Y, ptProjective.Z);

    this->ptProjective.X = ptProjective.X;
    this->ptProjective.Y = ptProjective.Y;
    this->ptProjective.Z = ptProjective.Z;

    // Add new position to the history buffer
    m_History[cxt->nID].push_front(ptProjective);
    // Keep size of history buffer
    if (m_History[cxt->nID].size() > m_nHistorySize)
        m_History[cxt->nID].pop_back();
    bShouldPrint = false;
}

// Handle destruction of an existing hand
void XnVPointDrawer::OnPointDestroy(XnUInt32 nID)
{
    // No need for the history buffer
    m_History.erase(nID);
    this->handFound = false;
}

#define MAX_DEPTH 10000
float g_pDepthHist[MAX_DEPTH];
unsigned int getClosestPowerOfTwo(unsigned int n)
{
    unsigned int m = 2;
    while(m < n) m<<=1;

    return m;
}
GLuint initTexture(void** buf, int& width, int& height)
{
    GLuint texID = 0;
    glGenTextures(1,&texID);

    width = getClosestPowerOfTwo(width);
    height = getClosestPowerOfTwo(height);
    *buf = new unsigned char[width*height*4];
    glBindTexture(GL_TEXTURE_2D,texID);

    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);

    return texID;
}

GLfloat texcoords[8];
void DrawRectangle(float topLeftX, float topLeftY, float bottomRightX, float bottomRightY)
{
    GLfloat verts[8] = {	topLeftX, topLeftY,
        topLeftX, bottomRightY,
        bottomRightX, bottomRightY,
        bottomRightX, topLeftY
    };
    glVertexPointer(2, GL_FLOAT, 0, verts);
    glDrawArrays(GL_TRIANGLE_FAN, 0, 4);

    glFlush();
}
void DrawTexture(float topLeftX, float topLeftY, float bottomRightX, float bottomRightY)
{
    glEnableClientState(GL_TEXTURE_COORD_ARRAY);
    glTexCoordPointer(2, GL_FLOAT, 0, texcoords);

    DrawRectangle(topLeftX, topLeftY, bottomRightX, bottomRightY);

    glDisableClientState(GL_TEXTURE_COORD_ARRAY);
}

void DrawDepthMap(const xn::DepthMetaData& dm)
{
    static bool bInitialized = false;
    static GLuint depthTexID;
    static unsigned char* pDepthTexBuf;
    static int texWidth, texHeight;

    float topLeftX;
    float topLeftY;
    float bottomRightY;
    float bottomRightX;
    float texXpos;
    float texYpos;

    if(!bInitialized)
    {
        XnUInt16 nXRes = dm.XRes();
        XnUInt16 nYRes = dm.YRes();
        texWidth =  getClosestPowerOfTwo(nXRes);
        texHeight = getClosestPowerOfTwo(nYRes);

        depthTexID = initTexture((void**)&pDepthTexBuf,texWidth, texHeight) ;

        bInitialized = true;

        topLeftX = nXRes;
        topLeftY = 0;
        bottomRightY = nYRes;
        bottomRightX = 0;
        texXpos =(float)nXRes/texWidth;
        texYpos  =(float)nYRes/texHeight;

        memset(texcoords, 0, 8*sizeof(float));
        texcoords[0] = texXpos, texcoords[1] = texYpos, texcoords[2] = texXpos, texcoords[7] = texYpos;

    }
    unsigned int nValue = 0;
    unsigned int nHistValue = 0;
    unsigned int nIndex = 0;
    unsigned int nX = 0;
    unsigned int nY = 0;
    unsigned int nNumberOfPoints = 0;
    XnUInt16 g_nXRes = dm.XRes();
    XnUInt16 g_nYRes = dm.YRes();

    unsigned char* pDestImage = pDepthTexBuf;

    const XnUInt16* pDepth = dm.Data();

    // Calculate the accumulative histogram
    memset(g_pDepthHist, 0, MAX_DEPTH*sizeof(float));
    for (nY=0; nY<g_nYRes; nY++)
    {
        for (nX=0; nX<g_nXRes; nX++)
        {
            nValue = *pDepth;

            if (nValue != 0)
            {
                g_pDepthHist[nValue]++;
                nNumberOfPoints++;
            }

            pDepth++;
        }
    }

    for (nIndex=1; nIndex<MAX_DEPTH; nIndex++)
    {
        g_pDepthHist[nIndex] += g_pDepthHist[nIndex-1];
    }
    if (nNumberOfPoints)
    {
        for (nIndex=1; nIndex<MAX_DEPTH; nIndex++)
        {
            g_pDepthHist[nIndex] = (unsigned int)(256 * (1.0f - (g_pDepthHist[nIndex] / nNumberOfPoints)));
        }
    }

    pDepth = dm.Data();
    {
        XnUInt32 nIndex = 0;
        // Prepare the texture map
        for (nY=0; nY<g_nYRes; nY++)
        {
            for (nX=0; nX < g_nXRes; nX++, nIndex++)
            {
                nValue = *pDepth;

                if (nValue != 0)
                {
                    nHistValue = g_pDepthHist[nValue];

                    pDestImage[0] = nHistValue;
                    pDestImage[1] = nHistValue;
                    pDestImage[2] = nHistValue;
                }
                else
                {
                    pDestImage[0] = 0;
                    pDestImage[1] = 0;
                    pDestImage[2] = 0;
                }

                pDepth++;
                pDestImage+=3;
            }

            pDestImage += (texWidth - g_nXRes) *3;
        }
    }
    glBindTexture(GL_TEXTURE_2D, depthTexID);
    glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, texWidth, texHeight, 0, GL_RGB, GL_UNSIGNED_BYTE, pDepthTexBuf);

    // Display the OpenGL texture map
    glColor4f(0.5,0.5,0.5,1);

    glEnable(GL_TEXTURE_2D);
    DrawTexture(dm.XRes(),dm.YRes(),0,0);
    glDisable(GL_TEXTURE_2D);
}

void glPrintString(void *font, char *str)
{
    size_t i,l = strlen(str);

    for(i=0; i<l; i++)
    {
        glutBitmapCharacter(font,*str++);
    }
}
void DrawFrameID(XnUInt32 nFrameID)
{
    glColor4f(1,0,0,1);
    glRasterPos2i(20, 50);
    XnChar strLabel[20];
    XnUInt32 nWritten = 0;
    xnOSStrFormat(strLabel, 20, &nWritten, "%d", nFrameID);
    glPrintString(GLUT_BITMAP_HELVETICA_18, strLabel);
}


// Colors for the points
XnFloat Colors[][3] =
{
    {0.5,0.5,0.5},	// Grey
    {0,1,0},	// Green
    {0,0.5,1},	// Light blue
    {1,1,0},	// Yellow
    {1,0.5,0},	// Orange
    {1,0,1},	// Purple
    {1,1,1}		// White. reserved for the primary point
};
XnUInt32 nColors = 6;

XnBool XnVPointDrawer::IsTouching(XnUInt32 id) const
{
    for (std::list<XnUInt32>::const_iterator iter = m_TouchingFOVEdge.begin(); iter != m_TouchingFOVEdge.end(); ++iter)
    {
        if (*iter == id)
            return TRUE;
    }
    return FALSE;
}

void XnVPointDrawer::Draw() const
{
    std::map<XnUInt32, std::list<XnPoint3D> >::const_iterator PointIterator;

    // Go over each existing hand
    for (PointIterator = m_History.begin();
        PointIterator != m_History.end();
        ++PointIterator)
    {
        // Clear buffer
        XnUInt32 nPoints = 0;
        XnUInt32 i = 0;
        XnUInt32 Id = PointIterator->first;

        // Go over all previous positions of current hand
        std::list<XnPoint3D>::const_iterator PositionIterator;
        for (PositionIterator = PointIterator->second.begin();
            PositionIterator != PointIterator->second.end();
            ++PositionIterator, ++i)
        {
            // Add position to buffer
            XnPoint3D pt(*PositionIterator);
            m_pfPositionBuffer[3*i] = pt.X;
            m_pfPositionBuffer[3*i + 1] = pt.Y;
            m_pfPositionBuffer[3*i + 2] = 0;//pt.Z();
        }

        // Set color
        XnUInt32 nColor = Id % nColors;
        XnUInt32 nSingle = GetPrimaryID();
        if (Id == GetPrimaryID())
            nColor = 6;
        // Draw buffer:
        glColor4f(Colors[nColor][0],
                Colors[nColor][1],
                Colors[nColor][2],
                1.0f);
        glPointSize(2);
        glVertexPointer(3, GL_FLOAT, 0, m_pfPositionBuffer);
        glDrawArrays(GL_LINE_STRIP, 0, i);


        if (IsTouching(Id))
        {
            glColor4f(1.0f, 0.0f, 0.0f, 1.0f);
        }
        glPointSize(8);
        glDrawArrays(GL_POINTS, 0, 1);
        glFlush();
    }
}
void XnVPointDrawer::SetTouchingFOVEdge(XnUInt32 nID)
{
    m_TouchingFOVEdge.push_front(nID);
}

// Handle a new Message
void XnVPointDrawer::Update(XnVMessage* pMessage)
{
    // PointControl's Update calls all callbacks for each hand
    XnVPointControl::Update(pMessage);

    if (m_bDrawDM)
    {
        // Draw depth map
        xn::DepthMetaData depthMD;
        m_DepthGenerator.GetMetaData(depthMD);
        DrawDepthMap(depthMD);
    }

    if (m_bFrameID)
    {
        // Print out frame ID
        xn::DepthMetaData depthMD;
        m_DepthGenerator.GetMetaData(depthMD);
        DrawFrameID(depthMD.FrameID());
    }

    // Draw hands
    Draw();
    m_TouchingFOVEdge.clear();
}

void PrintSessionState(SessionState eState)
{
    glColor4f(1,0,1,1);
    glRasterPos2i(20, 20);
    XnChar strLabel[XN_FILE_MAX_PATH];
    XnUInt32 nWritten = 0;

    switch (eState)
    {
    case IN_SESSION:
        xnOSStrFormat(strLabel, XN_FILE_MAX_PATH, &nWritten, "Tracking hands"); break;
    case NOT_IN_SESSION:
        xnOSStrFormat(strLabel, XN_FILE_MAX_PATH, &nWritten, "Perform click or wave gestures to track hand"); break;
    case QUICK_REFOCUS:
        xnOSStrFormat(strLabel, XN_FILE_MAX_PATH, &nWritten, "Raise your hand for it to be identified, or perform click or wave gestures"); break;
    }

    glPrintString(GLUT_BITMAP_HELVETICA_18, strLabel);
}
